#include "getMatch.h"


void GetMatch::GetInliers(const vector<vector<DMatch>> &dmatches, vector<int> &idx,double _ratio,double _distance)
{
    idx.clear();
	if (_distance == -1)
	{
		for (int i = 0; i < dmatches.size(); ++i)
		{
			if (dmatches[i][0].distance < _ratio*dmatches[i][1].distance)
			{
				idx.push_back(i);
			}
		}
	}
	else
	{
		for (int i = 0; i < dmatches.size(); ++i)
		{
			if (dmatches[i][0].distance < _distance&&dmatches[i][0].distance < _ratio*dmatches[i][1].distance)
			{
				idx.push_back(i);
			}
		}
	}

}


void GetMatch::GetInliersRadius(const vector<KeyPoint> &query, const vector<KeyPoint> &train, const vector<vector<DMatch>> &dmatches,vector<int>& idx, double ratio, double distance, double squareDistance)
{
	idx.clear();
	double dx, dy;
	for (int i = 0; i < dmatches.size(); ++i)
	{
		if (dmatches[i][0].distance < distance&&dmatches[i][0].distance < ratio*dmatches[i][1].distance)
		{
			dx = query[dmatches[i][0].queryIdx].pt.x - train[dmatches[i][0].trainIdx].pt.x;
			dy = query[dmatches[i][0].queryIdx].pt.y - train[dmatches[i][0].trainIdx].pt.y;
			if ((dx*dx+dy*dy)<squareDistance)
				idx.push_back(i);
		}
	}
}

void GetMatch::setXYZ(const vector<Feature*>&query,const vector<Feature*>&train,const vector<DMatch>&dmatches)
{
    for(int i=0;i<query.size();++i)
    {
        query[i]->pointXYZ = nullptr;
    }
    for (int i=0; i<dmatches.size(); ++i) {
        query[dmatches[i].queryIdx]->pointXYZ = train[dmatches[i].trainIdx]->pointXYZ;
    }
}

void GetMatch::GetInliers(const vector<DMatch> &dmatch, vector<int> &idx,double _distance)
{
    idx.clear();
	for (int i = 0; i < dmatch.size(); ++i)
	{
		if (dmatch[i].distance < _distance&&dmatch[i].distance>=0)
		{
			idx.push_back(i);
		}
	}
}

void GetMatch::GetDMatch(const vector<vector<DMatch>> &dmatches, const vector<int>&idx, vector<DMatch> &dmatch)
{
    dmatch.clear();
    dmatch.reserve(idx.size());
    
	for (int i = 0; i < idx.size();++i)
	{
		dmatch.push_back(dmatches[idx[i]][0]);
	}
}

void GetMatch::preparePoint2D(const vector<markerAR::Feature*> &query, const vector<markerAR::Feature*> train, const vector<DMatch> &dmatch, vector<Point2f> &query2D, vector<Point2f> &train2D)
{
    query2D.clear();
    train2D.clear();
    query2D.reserve(dmatch.size());
    train2D.reserve(dmatch.size());
    for (int j = 0; j < dmatch.size();++j)
    {
        auto &i = dmatch[j];
        if (i.queryIdx < 0)
            continue;
        auto &p1 =query[i.queryIdx]->_pos;
        
        auto &p2 =train[i.trainIdx]->_pos;
        query2D.push_back(Point2f(p1.x(),p1.y()));
        train2D.push_back(Point2f(p2.x(),p2.y()));
    }
}

void GetMatch::preparePoint2D(const vector<markerAR::Feature*> &query, const vector<markerAR::Feature*> train, const vector<DMatch> &dmatch, vector<Eigen::Vector2d> &query2D, vector<Eigen::Vector2d> &train2D)
{
    query2D.clear();
    train2D.clear();
    query2D.reserve(dmatch.size());
    train2D.reserve(dmatch.size());
    for (int j = 0; j < dmatch.size();++j)
    {
        auto &i = dmatch[j];
        if (i.queryIdx < 0)
            continue;
        auto &p1 =query[i.queryIdx]->_pos;
        
        auto &p2 =train[i.trainIdx]->_pos;
        query2D.push_back(p1);
        train2D.push_back(p2);
    }
}

void GetMatch::preparePoint2D(const vector<KeyPoint> &query, const vector<KeyPoint> train, const vector<DMatch> &dmatch, vector<Point2f> &query2D, vector<Point2f> &train2D)
{
    query2D.clear();
    train2D.clear();
    query2D.reserve(dmatch.size());
    train2D.reserve(dmatch.size());
	for (int j = 0; j < dmatch.size();++j)
	{
		auto &i = dmatch[j];
		if (i.queryIdx < 0)
			continue;
		query2D.push_back(query[i.queryIdx].pt);
		train2D.push_back(train[i.trainIdx].pt);
	}
}




void GetMatch::prepare3D_2D(const vector<Point3f> &trainP3D, const vector<KeyPoint> &queryKpts, const vector<DMatch> &dmatch, vector<Point3f> &train3d, vector<Point2f> &query2d)
{
	train3d.clear();
	query2d.clear();
	train3d.reserve(dmatch.size());
	query2d.reserve(dmatch.size());
	for (size_t j = 0; j < dmatch.size(); j++)
	{
		auto &i = dmatch[j];
		if (i.queryIdx < 0)
			continue;
		train3d.push_back(trainP3D[i.trainIdx]);
		query2d.push_back(queryKpts[i.queryIdx].pt);
	}
}



